/*
 * =====================================================================================
 *
 *       Filename:  rotation3d.cpp
 *
 *    Description:  
 *
 *        Version:  1.0
 *        Created:  2009年07月14日 16时59分26秒
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Ying Wang (WY), ywang@nlpr.ia.ac.cn
 *        Company:  Institute of Automation, Chinese Academy of Sciences
 *
 * =====================================================================================
 */

#include "rotation3d.h"

UnitQuaternion :: UnitQuaternion (double xx, double yy, double zz, double ww)
	: x(xx)
	, y(yy)
	, z(zz)
	, w(ww)
{
	if( fabs(x*x + y*y + z*z +w*w - 1) > 1e-10 )
	{
		std::cerr<<"It is not a unit quaternion!"<<std::endl;
	}
}

UnitQuaternion& UnitQuaternion :: operator = (const UnitQuaternion &uq)
{
	if ( this != &uq )
	{
		x = uq.x;
		y = uq.y;
		z = uq.z;
		w = uq.w;
	}
	return *this;
}

UnitQuaternion UnitQuaternion :: operator * (const UnitQuaternion &ruq) const
{
	return UnitQuaternion( w*ruq.x + x*ruq.w + y*ruq.z - z*ruq.y,
			       w*ruq.y + y*ruq.w + z*ruq.x - x*ruq.z,
			       w*ruq.z + z*ruq.w + x*ruq.y - x*ruq.y
			     );
}
NCmatrix<double> UnitQuaternion :: rotationMatrix() const
{
	NCmatrix<double> rotation(3,3);
	rotation[0][0] = 1-2*(y*y+z*z); rotation[0][1] = 2*(x*y-z*w);   rotation[0][2] = 2*(x*z+y*w);
	rotation[1][0] = 2*(x*y+z*w);   rotation[1][1] = 1-2*(x*x+z*z);	rotation[1][2] = 2*(y*z-x*w);
	rotation[2][0] = 2*(x*z-y*w);   rotation[2][1] = 2*(y*z+x*w);   rotation[2][2] = 1-2*(x*x+y*y);
	return rotation;
}
UnitQuaternion UnitQuaternion :: fromAxisAngle (double vx, double vy, double vz, double theta)
{
	double norm = sqrt(vx*vx + vy*vy + vz*vz);
	double ss = sin(theta/2);
	double cs = cos(theta/2);

	return UnitQuaternion(vx/norm*ss,vy/norm*ss,vz/norm*ss,cs);
}	
